#include "bga/graphics/Camera.h"
#include <GL/glew.h>
#include <iostream>
#include "bga/math/Matrix.h"
#include <cmath>
#include <sstream>

using namespace std;

namespace bga
{

Camera::Camera(const Vector3d& position, CameraType::Type type) : direction(1.f, 0.0,  0.0, 0.0),
                                                                    position(0.f, position),
                                                                    type(type)
{
    this->direction.normalize();

    //this->left=-0.09;
    //this->right=0.09;
    this->near=1;
    this->far=1000;
    //this->bottom=-0.09;
    this->top=1.f;

    glUpdateProjectionMatrix();
}

Camera::~Camera()
{
}


void Camera::setPosition(const Quaternion& position)
{
    this->position = position;
}

const Quaternion& Camera::getDirection() const
{
    return this->direction;
}

void Camera::setDirection(const Quaternion& direction)
{
    this->direction = direction;
}

CameraType::Type Camera::getType() const
{
    return this->type;
}

void Camera::look()
{
    Vector3d v = (this->direction.getConjugation() * this->getOrientation() * this->direction).toVector3D();
    //cout << v.getX() << ";" << v.getY() << ";" << v.getZ() << "       " << endl;

    buildTransformationMatrix();
    buildProjectionMatrix();

    ///glMatrixMode(GL_MODELVIEW);
    //glLoadMatrixf(this->mat_modelView);
    //glLoadMatrixf(this->modelViewMatrix.getArray());

}

Matrix4f* Camera::getViewMatrix()
{
    //return mat_modelView;
    return &this->modelViewMatrix/*.getArray()*/;
}

Matrix4f* Camera::getProjectionMatrix()
{
    //return mat_projection;
    return &this->projectionMatrix/*.getArray()*/;
}

void Camera::initializeQuaternionDirection(const Vector3d& eyePosition3D, const Vector3d& center3D, const Vector3d& upVector3D){
    #ifdef DEBUG
    cout<<"eyePosition="<<eyePosition3D<<" center3D="<<center3D<<" upVector3D="<<upVector3D<<endl;
    #endif

    Vector3d forward = center3D - eyePosition3D;


    forward.normalize();
    #ifdef DEBUG
    cout<<"forward = center3D-eyePosition3D = "<<forward<<endl;
    #endif


    Vector3d side = Vector3d::crossProduct(forward,upVector3D);
    side.normalize();

    /// Reverse side if wrong side



    #ifdef DEBUG
    cout<<"side=forward*upVector3D = "<<side<<endl;
    #endif

    Vector3d up=Vector3d::crossProduct(side,forward);
    up.normalize();
    #ifdef DEBUG
    cout<<"up=side*forward = "<<up<<endl;
    #endif

    //float matrix[16];
    Matrix4f matrix;

    /*matrix[0]=side.getX();
    matrix[1]=side.getY();
    matrix[2]=side.getZ();
    matrix[3]=0;
    matrix[4]=up.getX();
    matrix[5]=up.getY();
    matrix[6]=up.getZ();
    matrix[7]=0;
    matrix[8]=-forward.getX();
    matrix[9]=-forward.getY();
    matrix[10]=-forward.getZ();
    matrix[11]=0;
    matrix[12]=0;
    matrix[13]=0;
    matrix[14]=0;
    matrix[15]=1;*/

    matrix(0, 0) = side.getX();
    matrix(0, 1) = side.getY();
    matrix(0, 2) = side.getZ();
    matrix(0, 3) = 0;
    matrix(1, 0) = up.getX();
    matrix(1, 1) = up.getY();
    matrix(1, 2) = up.getZ();
    matrix(1, 3) = 0;
    matrix(2, 0) = -forward.getX();
    matrix(2, 1) = -forward.getY();
    matrix(2, 2) = -forward.getZ();
    matrix(2, 3) = 0;
    matrix(3, 0) = 0;
    matrix(3, 1) = 0;
    matrix(3, 2) = 0;
    matrix(3, 3) = 1;



    #ifdef DEBUG
    cout<<endl;
    cout<<"la matrice"<<endl;
    cout<<" '/' side[0]   up[0]   -forward[0] 0   '\'"<<endl;
    cout<<" '|' side[1]   up[1]   -forward[1] 0   '|'"<<endl;
    cout<<" '|' side[2]   up[2]   -forward[2] 0   '|'"<<endl;
    cout<<" '\' 0         0       0           1   '/'"<<endl;
    //Matrix4f::drawMatrix(matrix);
    cout << matrix.toString() << endl;
    #endif

    //this->setDirection(matrixToQuaternion(matrix));
    this->setDirection(Matrix3f::matrixToQuaternion(matrix.getArray()));

    #ifdef DEBUG
    cout << this->toString() << endl;
    #endif
}


void Camera::buildProjectionMatrix()
{
    //this->mat_projection[0] = 2 * this->near / (this->right-this->left);
    /*this->mat_projection[0] = this->near;
    this->mat_projection[1] = 0.f;
    this->mat_projection[2] = 0.f;
    this->mat_projection[3] = 0.f;
    this->mat_projection[4] = 0.f;
    //this->mat_projection[5] = 2 * this->near / (this->top-this->bottom);
    this->mat_projection[5] = this->near / this->top;
    this->mat_projection[6] = 0.f;
    this->mat_projection[7] = 0.f;
    //this->mat_projection[8] = (this->right+this->left)/(this->right-this->left);
    this->mat_projection[8] = 0;
    //this->mat_projection[9] = (this->top+this->bottom)/(this->top-this->bottom);
    this->mat_projection[9] = 0;
    this->mat_projection[10] = -(this->far+this->near)/(this->far-this->near);
    this->mat_projection[11] = -1.f;
    this->mat_projection[12] = 0;
    this->mat_projection[13] = 0;
    this->mat_projection[14] = -(2*this->near*this->far)/(this->far-this->near);
    this->mat_projection[15] = 0;*/

    /// Define in column
    this->projectionMatrix(0, 0) = this->near;
    this->projectionMatrix(0, 1) = 0.f;
    this->projectionMatrix(0, 2) = 0.f;
    this->projectionMatrix(0, 3) = 0.f;
    this->projectionMatrix(1, 0) = 0.f;
    //this->mat_projection(5) = 2 * this->near / (this->top-this->bottom);
    this->projectionMatrix(1, 1) = this->near / this->top;
    this->projectionMatrix(1, 2) = 0.f;
    this->projectionMatrix(1, 3) = 0.f;
    //this->mat_projection(8) = (this->right+this->left)/(this->right-this->left);
    this->projectionMatrix(2, 0) = 0;
    //this->mat_projection(9) = (this->top+this->bottom)/(this->top-this->bottom);
    this->projectionMatrix(2, 1) = 0;
    this->projectionMatrix(2, 2) = -(this->far+this->near)/(this->far-this->near);
    this->projectionMatrix(2, 3) = -1.f;
    this->projectionMatrix(3, 0) = 0;
    this->projectionMatrix(3, 1) = 0;
    this->projectionMatrix(3, 2) = -(2*this->near*this->far)/(this->far-this->near);
    this->projectionMatrix(3, 3) = 0;


}

void Camera::glUpdateProjectionMatrix()
{
    buildProjectionMatrix();
    ///glMatrixMode(GL_PROJECTION);
    //glLoadMatrixf(this->mat_projection);
    ///glLoadMatrixf(this->projectionMatrix.getArray());
}

void Camera::setAspectRatio( double value )
{
    this->angle = value;
    glUpdateProjectionMatrix();
}

void Camera::setPlanes( double near_plane, double far_plane )
{
    this->near = near_plane;
    this->far = far_plane;
    glUpdateProjectionMatrix();
}

void Camera::setFOV( double angle )
{
    this->fov = angle;
    glUpdateProjectionMatrix();
}

string Camera::toString() const
{
    ostringstream stream;
    /*//affiche les quaternion
    stream <<"Qposition: "<< this->position.toString() <<endl;
    stream <<"Qdirection: "<< this->direction.toString() <<endl;*/

    /*//afficher les angles d'euler
    float q0=this->getDirection().getS();
    float q1=this->getDirection().getX();
    float q2=this->getDirection().getY();
    float q3=this->getDirection().getZ();

    float a2=atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2));
    float b2=asin(2*(q0*q2-q3*q1));
    float c2=atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3));
    stream<<"a2="<<a2<<" b2="<<b2<<" c2="<<c2<<endl;*/

    cout<<"Orientation: "<<this->getOrientation()<<endl;
    cout<<"Position: "<<this->getPosition()<<endl;

    return stream.str();
}


}
